/**
 * 
*/

import processing.serial.*;

Serial myPort;  // Create object from Serial class

float [] Q = new float [4];
float [] Euler = new float [3]; // psi, theta, phi
int lf = 10; // 10 is '\n' in ASCII
byte[] inBuffer = new byte[22]; // this is the number of chars on each line from the Arduino (including /r/n)

PFont font;
final int VIEW_SIZE_X = 600, VIEW_SIZE_Y = 600;

void setup() 
{
  size(VIEW_SIZE_X, VIEW_SIZE_Y, P3D);
  myPort = new Serial(this, "COM13", 115200);  
  // The font must be located in the sketch's "data" directory to load successfully
  font = loadFont("CourierNew36.vlw"); 
  delay(100);
  myPort.clear();
  //myPort.write("start");
}

float decodeFloat(String inString) {
  byte [] inData = new byte[4];

  inString = inString.substring(2, 10); // discard the leading "f:"
  inData[0] = (byte) unhex(inString.substring(0, 2));
  inData[1] = (byte) unhex(inString.substring(2, 4));
  inData[2] = (byte) unhex(inString.substring(4, 6));
  inData[3] = (byte) unhex(inString.substring(6, 8));
      
  int intbits = (inData[3] << 24) | ((inData[2] & 0xff) << 16) | ((inData[1] & 0xff) << 8) | (inData[0] & 0xff);
  return Float.intBitsToFloat(intbits);
}

void readQ() {
  if(myPort.available() >= 18) {
    String inputString = myPort.readStringUntil((int) '\n');
    print(inputString);
    if (inputString != null && inputString.length() > 0) {
      String [] inputStringArr = split(inputString, ",");
      if(inputStringArr.length == 4) {
        Q[0] = decodeFloat(inputStringArr[0]);
        Q[1] = decodeFloat(inputStringArr[1]);
        Q[2] = decodeFloat(inputStringArr[2]);
        Q[3] = decodeFloat(inputStringArr[3]);
      } else {
        println(inputStringArr.length);
      }
    }
  }
}

void buildBoxShape() {
  //box(60, 10, 40);
  noStroke();
  beginShape(QUADS);
  
  //Z+ (to the drawing area)
  fill(#00ff00); // green
  vertex(-30, -5, 20);
  vertex(30, -5, 20);
  vertex(30, 5, 20);
  vertex(-30, 5, 20);
  
  //Z-
  fill(#0000ff); // blue
  vertex(-30, -5, -20);
  vertex(30, -5, -20);
  vertex(30, 5, -20);
  vertex(-30, 5, -20);
  
  //X-
  fill(#ff0000); // red
  vertex(-30, -5, -20);
  vertex(-30, -5, 20);
  vertex(-30, 5, 20);
  vertex(-30, 5, -20);
  
  //X+
  fill(#ffff00); // yellow
  vertex(30, -5, -20);
  vertex(30, -5, 20);
  vertex(30, 5, 20);
  vertex(30, 5, -20);
  
  //Y-
  fill(#ff00ff); // magenta
  vertex(-30, -5, -20);
  vertex(30, -5, -20);
  vertex(30, -5, 20);
  vertex(-30, -5, 20);
  
  //Y+
  fill(#00ffff); // cyan
  vertex(-30, 5, -20);
  vertex(30, 5, -20);
  vertex(30, 5, 20);
  vertex(-30, 5, 20);
  
  endShape();
}

void drawCube() {  
  pushMatrix();
    translate(300, 350, 0);
    scale(5,5,5);
    
    // please, don't ask me why these angles are negated
    rotateZ(-Euler[2]);
    rotateX(-Euler[1]);
    rotateY(-Euler[0]);
    buildBoxShape();
    
  popMatrix();
}

void draw() {  
  readQ();
  quaternionToEuler(Q, Euler);
  background(#000000);
  fill(#ffffff);
  textFont(font, 16);
  //float temp_decoded = 35.0 + ((float) (temp + 13200)) / 280;
  //text("temp:\n" + temp_decoded + " C", 350, 250);
  text("Q:\n" + Q[0] + "\n" + Q[1] + "\n" + Q[2] + "\n" + Q[3], 20, 50);
  text("Euler Angles:\npsi  : " + degrees(Euler[0]) + "\ntheta: " + degrees(Euler[1]) + "\nphi  : " + degrees(Euler[2]), 200, 50);
  drawCube();
}

// See Sebastian O.H. Madwick report "An efficient orientation filter for inertial and intertial/magnetic sensor arrays
void quaternionToEuler(float [] q, float [] euler) {
  euler[0] = atan2(2 * q[1] * q[2] - 2 * q[0] * q[3], 2 * q[0]*q[0] + 2 * q[1] * q[1] - 1); // psi
  euler[1] = -asin(2 * q[1] * q[3] + 2 * q[0] * q[2]); // theta
  euler[2] = atan2(2 * q[2] * q[3] - 2 * q[0] * q[1], 2 * q[0] * q[0] + 2 * q[3] * q[3] - 1); // phi
}

